#include "serial.h"

//@brief:linux下的串口通信类的成员函数。
//       open_port()成员函数可以打开一个串口，set_opt()更改参数。
//@example:open_port("/dev/ttyUSB0");
//         set_opt(115200, 8, 'N', 1);
Serialport::Serialport(string port)
{
    open_port(port);
    set_opt();
}

// Serialport下的成员函数open_port()的实现；
int Serialport::open_port(string port)
{
    // char *dev[]={"/dev/ttyS0","/dev/ttyS1","/dev/ttyS2"};
    // fd = open( "/dev/ttyS0", O_RDWR|O_NOCTTY|O_NDELAY);
    // fd = open( "/dev/ttyUSB0", O_RDWR|O_NOCTTY|O_NDELAY);
    fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (-1 == fd)
    {
        printf("\n\e[31m\e[1m ERROR:打开串口%s失败 \e[0m\n\n", port.c_str());
        return -1;
    }
    else
    {
        fcntl(fd, F_SETFL, 0);
    }

    if (fcntl(fd, F_SETFL, 0) < 0)
        printf("fcntl failed!\n");
    else
        printf("fcntl=%d\n", fcntl(fd, F_SETFL, 0));

    if (isatty(STDIN_FILENO) == 0)
        printf("standard input is not a terminal device\n");
    else
        printf("isatty success!\n");

    // printf("fd-open=%d\n",fd);

    return fd;
}

/*设置串口属性：
fd: 文件描述符
nSpeed: 波特率
nBits: 数据位
nEvent: 奇偶校验
nStop: 停止位*/

int Serialport::set_opt(int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio, oldtio;
    if (tcgetattr(fd, &oldtio) != 0)
    {
        perror("SetupSerial error");
        return -1;
    }
    bzero(&newtio, sizeof(newtio));
    newtio.c_cflag |= CLOCAL | CREAD;
    newtio.c_cflag &= ~CSIZE;
    switch (nBits)
    {
    case 7:
        newtio.c_cflag |= CS7;
        break;
    case 8:
        newtio.c_cflag |= CS8;
        break;
        // case 9:
        // newtio.c_cflag |= CS9;
        // break;
    }
    switch (nEvent)
    {
    case 'O':
        newtio.c_cflag |= PARENB;
        newtio.c_cflag |= PARODD;
        newtio.c_iflag |= (INPCK | ISTRIP);
        break;
    case 'E':
        newtio.c_iflag |= (INPCK | ISTRIP);
        newtio.c_cflag |= PARENB;
        newtio.c_cflag &= ~PARODD;
        break;
    case 'N':
        newtio.c_cflag &= ~PARENB;
        break;
    }
    switch (nSpeed)
    {
    case 2400:
        cfsetispeed(&newtio, B2400);
        cfsetospeed(&newtio, B2400);
        break;
    case 4800:
        cfsetispeed(&newtio, B4800);
        cfsetospeed(&newtio, B4800);
        break;
    case 9600:
        cfsetispeed(&newtio, B9600);
        cfsetospeed(&newtio, B9600);
        break;
    case 115200:
        cfsetispeed(&newtio, B115200);
        cfsetospeed(&newtio, B115200);
        break;
    default:
        cfsetispeed(&newtio, B9600);
        cfsetospeed(&newtio, B9600);
        break;
    }

    if (nStop == 1)
        newtio.c_cflag &= ~CSTOPB;
    else if (nStop == 2)
        newtio.c_cflag |= CSTOPB;

    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 0;
    tcflush(fd, TCIFLUSH);

    if ((tcsetattr(fd, TCSANOW, &newtio)) != 0)
    {
        perror("com set error");
        return -1;
    }
    printf("Serial port set done!\n");
    return 0;
}

bool Serialport::send(char *str)
{
    buffer = str;
    if (fd < 0 || write(fd, buffer, 1) < 0)
    {
        perror("\n\e[31m\e[1m ERROR:串口通信失败 \e[0m\n\n");

        return false;
    }

    return true;
}

bool Serialport::sendAngle(float *angle) //弧度制
{
    unsigned char send_bytes[] = {0xFF, 0x00, 0x00, 0x00, 0x00, 0xFE};
    if (angle == NULL)
    {
        
        if (6 == write(fd, send_bytes, 6))
            return true;
        return false;
    }

    short *data_ptr = (short *)(send_bytes + 1);
    data_ptr[0] = (angle[0]+PI) * 10000;
    data_ptr[1] = (angle[1]+PI) * 10000;
    if (6 == write(fd, send_bytes, 6))
        return true;
    return false;
}

bool Serialport::sendSpeed(float speedYaw, float speedPitch)
{
    char send_num = 0;
    unsigned char *ptr;
    memset(tmpchar, 0, sizeof(tmpchar)); //缓冲区置空

    tmpchar[0] = 0xA6; //设置起始标志位
    tmpchar[1] = 0x6A;

    ptr = (unsigned char *)&speedYaw;
    tmpchar[2] = *ptr;
    tmpchar[3] = *(ptr + 1);
    tmpchar[4] = *(ptr + 2);
    tmpchar[5] = *(ptr + 3);

    ptr = (unsigned char *)&speedPitch;
    tmpchar[6] = *ptr;
    tmpchar[7] = *(ptr + 1);
    tmpchar[8] = *(ptr + 2);
    tmpchar[9] = *(ptr + 3);

    tmpchar[10] = 0xAA;
    tmpchar[11] = 0xAA;

    for (send_num = 0; send_num < sizeof(tmpchar); send_num++)
    {
        if (!send(tmpchar + send_num))
        {
            return false;
        }
    }
    std::cout << "send successfully" << std::endl;
    return true;
}

bool Serialport::sendXYZ(double *xyz)
{
    unsigned char send_bytes[] = {0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE};
    if (NULL == xyz)
    {
        if (8 == write(fd, send_bytes, 8))
            return true;
        return false;
    }
    short *data_ptr = (short *)(send_bytes + 1);
    data_ptr[0] = (short)xyz[0];
    data_ptr[1] = (short)xyz[1];
    data_ptr[2] = (short)xyz[2];
    if (8 == write(fd, send_bytes, 8))
        return true;
    return false;
}

bool Serialport::sendXY(double *xy)
{
    unsigned char send_bytes[] = {0xFF, 0x00, 0x00, 0x00, 0x00, 0xFE};
    if (NULL == xy)
    {
        if (6 == write(fd, send_bytes, 6))
            return true;
        return false;
    }
    short *data_ptr = (short *)(send_bytes + 1);
    data_ptr[0] = (short)xy[0];
    data_ptr[1] = (short)xy[1];
    if (6 == write(fd, send_bytes, 6))
        return true;
    return false;
}

void Serialport::clear()
{
    tcflush(fd,TCIFLUSH);
}
// 0xA5 | 0x5A | hitmode | 0xff
void Serialport::readMode(int &carMode)
{
    int bytes;
    ioctl(fd, FIONREAD, &bytes);
    if (bytes == 0)
        return;
    // cout<<"get read mode!"<<endl;
    bytes = read(fd, rData, 6);
    if (rData[0] == 0xA5)
    {
        carMode = rData[2];
        printf("receive mode:%d\r\n", carMode);
    }
    /*ioctl(fd, FIONREAD, &bytes);
    if(bytes>0)
    {
        read(fd,rData,bytes);
    }
    cout << "readMode end!!!\n" << endl;*/
}

//接收标志两字节 0xA6 0x6A
bool Serialport::readStatusTo(DataPack &datapack)
{
    int bytes;
    ioctl(fd, FIONREAD, &bytes);
    if (bytes == 0)
        return false;

    bytes = read(fd, rData,7);
    unsigned char *ptr = (unsigned char *)rData + 1;
    if (rData[0] == 0xFF&&rData[6]==0xFE)
    {
        datapack.euler_angle[0]=0;
        //datapack.euler_angle[0]=(0.0001f*((float)(ptr[1]<<8|ptr[0])))-PI;
        datapack.euler_angle[1]=(0.0001f*((float)(ptr[0]<<8|ptr[1])))-PI;

        datapack.euler_angle[2]=0;
        datapack.angular_velocity[1]=(0.0001f*((float)(ptr[2]<<8|ptr[3])))-PI;
        //datapack.euler_angle[2]=(0.0001f*((float)(ptr[5]<<8|ptr[4])))-PI;
        // datapack.angular_velocity[0]=(0.0001f*((float)(ptr[7]<<8|ptr[6])))-PI;
        // datapack.angular_velocity[1]=(0.0001f*((float)(ptr[9]<<8|ptr[8])))-PI;
        // datapack.angular_velocity[2]=(0.0001f*((float)(ptr[11]<<8|ptr[10])))-PI;
        datapack.enemyColor=(char)ptr[4];
        datapack.isRead=true;
        return true;
    }
    datapack.isRead = false;
    return false;
}


Serialport::~Serialport()
{
    close(fd);
}